#include "kinematics.h"
#include "../control.h"
#include "robot_kdl.hpp"
#include <chain.hpp>
#include <chainfksolverpos_recursive.hpp>
#include <chainidsolver_recursive_newton_euler.hpp>
#include <frames_io.hpp>
#include <kinfam_io.hpp>
#include <math.h>
extern STRUCT_CONTROL Ct;
using namespace KDL;
static int Kinematic_CheckLimit(double posit[9]);
ROBOTS_KDL qx_kdl(QX_SCARA_V1);

/*check position limit*/
int Kinematic_CheckLimit(double posit[9]) {
    int i = 0;
    for (i = 0; i < MAX_JOINT; i++) {
        if (Ct.conf.joint_enable[i] == 1) {
            if (posit[i] > Ct.conf.limit_abs.limit_h[i] ||
                posit[i] < Ct.conf.limit_abs.limit_l[i]) {
                return -1;
            }
        }
    }
    return 0;
}

/*set robot physical construction parameter*/
// int Kinematic_SetMachinePara(STRUCT_DH dh[6], STRUCT_TOOL_DESCRIPTION tool) {
//     if (SCARA == Ct.conf.machine_struct) {
//         // Scara_Kinematic_SetMachinePara(dh, tool);
//     }
//     return 0;
// }

/*tool's physical construction parameter*/
int Kinematic_SetTool(STRUCT_TOOL_DESCRIPTION tool) {
    if (SCARA == Ct.conf.machine_struct) {
        Frame frame;
        frame.p = Vector(tool.x, tool.y, tool.z);
        frame.M = Rotation::RPY(tool.rx, tool.ry, tool.rz);
        qx_kdl.SetEndEffector(&frame);
    }
    return 0;
}

/*kinematics hand*/
int Kinematic_HandForward(double posit[MAX_JOINT], STRUCT_POSE *pose, int *fkine_flag) {
    int ret = 0;
    if (SCARA == Ct.conf.machine_struct) {
        if (Kinematic_CheckLimit(posit) != 0) {
            *fkine_flag = -2;
            return -1;
        }
        // Scara_Kinemtaic_HandForward(posit, pose, fkine_flag);
        JntArray q(qx_kdl.GetJointNum());
        JntArray iq(qx_kdl.GetJointNum());
        q.data << posit[0], posit[1], posit[2], posit[3];
        Frame frame;
        RPY rpy;
        ret = qx_kdl.FkJointArray2FrameHand(&q, &frame, fkine_flag);
        qx_kdl.Frame2Rpy(&frame, &rpy);
        pose->x = frame.p.x();
        pose->y = frame.p.y();
        pose->z = frame.p.z();
        pose->rz = rpy.yaw;
        pose->rx = posit[4];
        pose->ry = posit[5];
        pose->u = posit[6];
        pose->v = posit[7];
        pose->w = posit[8];
        return ret;
    }
    return 0;
}

/*
clamp forword kinematics if fkine_flag
return -1:posit limit
*/
int Kinematic_ClampForward(double posit[MAX_JOINT], STRUCT_POSE *pose, int *fkine_flag) {
    int ret = 0;
    if (SCARA == Ct.conf.machine_struct) {
        if (Kinematic_CheckLimit(posit) != 0) {
            if (fkine_flag != NULL) *fkine_flag = -2;
            DError("faild inverkinemtic for posit abs limit");
            return -1;
        }
        JntArray q(qx_kdl.GetJointNum());
        JntArray iq(qx_kdl.GetJointNum());
        q.data << posit[0], posit[1], posit[2], posit[3];
        Frame frame;
        RPY rpy;
        ret = qx_kdl.FkJointArray2FrameEndEffector(&q, &frame, fkine_flag);
        qx_kdl.Frame2Rpy(&frame, &rpy);
        pose->x = frame.p.x();
        pose->y = frame.p.y();
        pose->z = frame.p.z();
        pose->rz = rpy.yaw;
        pose->rx = posit[4];
        pose->ry = posit[5];
        pose->u = posit[6];
        pose->v = posit[7];
        pose->w = posit[8];
        return ret;
    }
    return 0;
}
/*
inverse kinematics
ikine_fkag:inverse kinematic flag
c_posit:current posit
-1: limit
-2: ik_flag error
*/
int Kinematic_HandInverse(double posit[MAX_JOINT], STRUCT_POSE *pose, int ikine_flag, double c_posit[MAX_JOINT]) {
    int ret = 0;
    if (SCARA == Ct.conf.machine_struct) {
        double i_posit[2][4];
        ret = Kinematic_CheckIkFlagValid(ikine_flag);
        if (ret != 0) return -2;
        // if (Scara_Kinemtaic_HandInverse(i_posit, pose) != 0) return -1;
        JntArray iq(qx_kdl.GetJointNum());
        Frame frame;
        frame.p = Vector(pose->x, pose->y, pose->z);
        frame.M = Rotation::RPY(0, 0, pose->rz);
        ret = qx_kdl.IkFrame2JointArrayHand(NULL, &frame, ikine_flag, &iq);
        posit[0] = iq.data(0);
        posit[1] = iq.data(1);
        posit[2] = iq.data(2);
        posit[3] = iq.data(3);
        posit[4] = pose->rx;
        posit[5] = pose->ry;
        posit[6] = pose->u;
        posit[7] = pose->v;
        posit[8] = pose->w;
        return ret;
    }
    return 0;
}

//************************************
// Method:    Kinematic_ClampInverse
// FullName:  kinematics.cpp
// Access:    public
// Returns:   int32_t 0->�ɹ� -1->ʧ��
// Qualifier: �˶�ѧ���
// Parameter: uint8_t offset
// Parameter: uint8_t channel
// Parameter: STRUCT_DOF dof
//************************************
int Kinematic_ClampInverse(double posit[MAX_JOINT], STRUCT_POSE *pose, int ikine_flag, double c_posit[MAX_JOINT]) {
    int ret = 0;
    if (SCARA == Ct.conf.machine_struct) {
        double i_posit[2][4];
        ret = Kinematic_CheckIkFlagValid(ikine_flag);
        if (ret != 0) return -2;
        // if (Scara_Kinemtaic_ClampInverse(i_posit, pose) != 0) return -1;
        JntArray iq(qx_kdl.GetJointNum());
        Frame frame;
        frame.p = Vector(pose->x, pose->y, pose->z);
        frame.M = Rotation::RPY(0, 0, pose->rz);
        ret = qx_kdl.IkFrame2JointArrayEndEffector(NULL, &frame, ikine_flag, &iq);
        posit[0] = iq.data(0);
        posit[1] = iq.data(1);
        posit[2] = iq.data(2);
        posit[3] = iq.data(3);
        posit[4] = pose->rx;
        posit[5] = pose->ry;
        posit[6] = pose->u;
        posit[7] = pose->v;
        posit[8] = pose->w;
        return ret;
    }
    return 0;
}

/*
check ik_flag is valid 
it will return 0,else return -1
*/
int Kinematic_CheckIkFlagValid(int ik_flag) {
    if (SCARA == Ct.conf.machine_struct) {
        if (ik_flag == SCARA_LEFT_ARM || ik_flag == SCARA_RIGHT_ARM)
            return 0;
        else
            return -1;
    }
    return 0;
}
